package robot;

import javax.vecmath.Point2d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector2d;

import program.MovingObjects;
import program.MovingSensorBelt;
import program.RoboMap;
import program.RoboMap.Content;

import simbad.sim.Agent;
import simbad.sim.RangeSensorBelt;

/**
 * Base class for all robots
 * @date 15 maj 2012
 * @version 1.0
 */
public abstract class Robot extends Agent {
	
	public static final double ACCEPT_DISTANCE = 0.1;
	
	protected RangeSensorBelt sensors;
	private MovingSensorBelt mSensors;
	private double heading;
    protected RoboMap roboMap;
    private int index;
    private boolean finished;
    
    protected RoboLogger roboLogger = new RoboLogger();
     
    //Parameter
    public final int MIN_RANGE_SENSOR = 0,
    				MAX_RANGE_SENSOR = 10,
    				NUMBER_OF_SENSORS = 128,
    				VISIBLE_SENSOR = RangeSensorBelt.FLAG_SHOW_FULL_SENSOR_RAY;
	
    public Robot(Vector3d position, String name, MovingObjects mo) {
		super(position, name);
		
		this.heading = 0;
		this.roboMap = new RoboMap();
		
		//Add sensors
		this.sensors = new RangeSensorBelt(0, MIN_RANGE_SENSOR, MAX_RANGE_SENSOR,
				NUMBER_OF_SENSORS, RangeSensorBelt.TYPE_LASER,
				VISIBLE_SENSOR);
		this.sensors.setUpdateOnEachFrame(true);
		this.addSensorDevice(sensors, new Vector3d(0,0,0), 0);
		this.mSensors = new MovingSensorBelt(mo, this, sensors);
		this.finished = false;
	}
    
    public Robot(Vector3d position, String name, MovingObjects mo, int sensorRange) {
    	super(position, name);
		
		this.heading = 0;
		this.roboMap = new RoboMap();
		
		//Add sensors
		this.sensors = new RangeSensorBelt(0, MIN_RANGE_SENSOR, sensorRange,
				NUMBER_OF_SENSORS, RangeSensorBelt.TYPE_LASER,
				VISIBLE_SENSOR);
		this.sensors.setUpdateOnEachFrame(true);
		this.addSensorDevice(sensors, new Vector3d(0,0,0), 0);
		this.mSensors = new MovingSensorBelt(mo, this, sensors);
		this.finished = false;
    }
    
    public Robot(Vector3d position, String name, MovingObjects mo, int sensorRange, boolean assumeConcave, int concaveRange) {
    	super(position, name);
		
		this.heading = 0;
		this.roboMap = new RoboMap(concaveRange,assumeConcave);
		
		//Add sensors
		this.sensors = new RangeSensorBelt(0, MIN_RANGE_SENSOR, sensorRange,
				NUMBER_OF_SENSORS, RangeSensorBelt.TYPE_LASER, RangeSensorBelt.FLAG_SHOW_FULL_SENSOR_RAY);
		this.sensors.setUpdateOnEachFrame(true);
		this.addSensorDevice(sensors, new Vector3d(0,0,0), 0);
		this.mSensors = new MovingSensorBelt(mo, this, sensors);
		this.finished = false;
    }

	public Point2d getCurrentPosition() {
		Point3d p = new Point3d();
		getCoords(p);
		return new Point2d(p.x, p.z);
	}
	
	public boolean isFinished() {
		return finished;
	}

	public void setFinished(boolean finished) {
		this.finished = finished;
	}

	public int getIndex() {
		return index;
	}

	public void setIndex(int index) {
		this.index = index;
	}

	public double getHeading() {
		return heading;
	}
	
	public void printStatistics() {
		System.out.println("ROBO STATISTICS");
		System.out.println("------------------");
		System.out.println("------------------");
		System.out.print("Distance traveled: "+roboLogger.distanceTraveled+"\nPaths calculated: "+roboLogger.pathsCalculated+
				         "\nTotal targets in path: "+roboLogger.path.size());
		System.out.flush();
	}
	
    public void initBehavior () {
    	//Do not use
    }
    
    public abstract void performBehavior();
    
    /***
     * When run it checks all sensors and update roboMap accordingly.
     */
    protected void updateRoboMap () {
    	for (int i = 0; i < sensors.getNumSensors(); i++) {
    		if (!Double.isInfinite(sensors.getMeasurement(i))){
    			double angle  = getHeading() + sensors.getSensorAngle(i);
    			Point2d position = 
    					new Point2d ( Math.cos(angle) 
    									* sensors.getMeasurement(i) 
    									+ getCurrentPosition().x,
    								-1 * Math.sin(angle) 
    									* sensors.getMeasurement(i) 
    									+ getCurrentPosition().y);
    			
    			Point2d movingObject = mSensors.getPos(i);
    			if (mSensors.getPos(i) != null) {
    				roboMap.addMovingObject(movingObject, mSensors.getDir(i), getLifeTime());
    			} else  {
    				roboMap.setPosition(position, Content.OBJECT);
    			}
    		}
    	}
    }
    
    protected void rotateTowardsNextGoal(Point2d nextGoal) {
    	
    	double distance = getCurrentPosition().distance(nextGoal);
    	Vector2d targetVec = new Vector2d((nextGoal.x - getCurrentPosition().x), (nextGoal.y - getCurrentPosition().y)); 
    	
    	//first quadrant
    	if(targetVec.x >= 0 && targetVec.y >= 0) { 
    		double deg = Math.asin(targetVec.y/distance);
        	this.rotateY((2*Math.PI - deg) - heading);
        	heading = (2*Math.PI - deg);
    	}
    	//second quadrant
    	else if(targetVec.x >= 0 && targetVec.y <= 0) { 
    		double deg = Math.asin(-targetVec.y/distance);
    		this.rotateY(deg - heading);
        	heading = (deg);
    	}
    	//Third quadrant
    	else if(targetVec.x <= 0 && targetVec.y <= 0) { 
    		double deg = Math.asin(-targetVec.y/distance);
    		this.rotateY((Math.PI - deg) - heading);
        	heading = (Math.PI - deg);
    	}
    	// fourth quadrant
    	else { 
    		double deg = Math.asin(targetVec.y/distance);
    		this.rotateY((Math.PI + deg) - heading);
        	heading = (Math.PI + deg);
    	}
    }
    
    
}
